#ok though, need adjust for obstacle
import numpy as np
import matplotlib.pyplot as plt
import json

def load_map_from_json(json_file):
    """加载JSON地图数据"""
    with open(json_file, 'r') as f:
        data = json.load(f)
    return np.array(data['data']).reshape((data['height'], data['width']))

def manhattan_path_with_avoidance(grid, start, goal):
    """曼哈顿路径 + 静态避障"""
    path = [start]
    x, y = start
    gx, gy = goal

    # 横向移动（优先）
    step_x = 1 if gx > x else -1
    while x != gx:
        x += step_x
        if grid[y][x] != 0:  # 遇到障碍物
            # 尝试垂直绕行（向上或向下）
            if y + 1 < grid.shape[0] and grid[y + 1][x] == 0:  # 向下绕行
                path.append((x, y + 1))
                path.append((x, y))
            elif y - 1 >= 0 and grid[y - 1][x] == 0:  # 向上绕行
                path.append((x, y - 1))
                path.append((x, y))
            else:
                print(f"Path blocked at ({x}, {y})! Cannot bypass.")
                return []
        path.append((x, y))

    # 纵向移动
    step_y = 1 if gy > y else -1
    while y != gy:
        y += step_y
        if grid[y][x] != 0:  # 遇到障碍物
            # 尝试水平绕行（向左或向右）
            if x + 1 < grid.shape[1] and grid[y][x + 1] == 0:  # 向右绕行
                path.append((x + 1, y))
                path.append((x, y))
            elif x - 1 >= 0 and grid[y][x - 1] == 0:  # 向左绕行
                path.append((x - 1, y))
                path.append((x, y))
            else:
                print(f"Path blocked at ({x}, {y})! Cannot bypass.")
                return []
        path.append((x, y))

    return path

def plot_path(grid, path, start, goal):
    """可视化地图和路径"""
    plt.figure(figsize=(10, 10))
    plt.imshow(grid, cmap='gray_r', vmin=0, vmax=100)  # 0=黑（可通行），100=白（障碍）
    plt.plot(start[0], start[1], 'go', markersize=12, label='Start')
    plt.plot(goal[0], goal[1], 'ro', markersize=12, label='Goal')
    if path:
        plt.plot([p[0] for p in path], [p[1] for p in path], 'b-', linewidth=2, label='Path')
    plt.legend()
    plt.title("Manhattan Path with Static Obstacle Avoidance")
    plt.show()

if __name__ == "__main__":
    # 1. 加载地图
    grid = load_map_from_json("d:/F/Open Source/Prj_Robdog/xiaozhi-esp32-server/my_mcp/yolo8/ESP32_Slam/esp32_nav_map.json")

    # 2. 定义起点和终点, 用画图工具可看到pix number
    start = (150, 555)
    goal = (260, 500)

    # 3. 生成路径（含避障）
    path = manhattan_path_with_avoidance(grid, start, goal)

    # 4. 输出结果
    if not path:
        print("No valid path found!")
    else:
        print(f"Path found with {len(path)} steps:")
        for i, (x, y) in enumerate(path):
            print(f"Step {i + 1}: ({x}, {y})")

        # 5. 可视化
        plot_path(grid, path, start, goal)